﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace PayloadTracker.APRS.SoundModem
{
    public class Modulator
    {
        private FSK.ModemDeffinition m_Modem;
        public Modulator(FSK.ModemDeffinition modemdef)
        {
            m_Modem = modemdef;
        }

        public float[] Modulate(Byte[] bytes)
        {
            List<float> samples = new List<float>();

            var packed = BitPack(bytes);
            var nrzid = NRZIEncode(packed);

            //calculate the number of samples we need to produce per bit.
            float bitperiod = ((float)m_Modem.SampleRate) / ((float)m_Modem.BaudRate);
            //calculate our time step
            double step = 1 / (double)m_Modem.SampleRate;
            
            //initialize time.
            double t = 0;
            float freq = m_Modem.FreqMark;
            foreach (byte b in nrzid)
            {//for every byte
                
                for (int biti = 0; biti <  8; biti++)
                {
                    bool bit = ((b & (byte)(1 << biti)) != 0);
                    //for every bit
                    
                    //check to see if we are in a frequency transition.
                    float freqnew = (bit) ? m_Modem.FreqMark : m_Modem.FreqSpace;
                    if (freqnew != freq)
                    {//if we are perserve the phase
                        t = (freq / freqnew) * t;
                        freq = freqnew;
                    }

                    Logger.LOG.Write(bit ? "1" : "0");


                    
                    for (int i = 0; i < bitperiod; i++,t += step)
                    {
                        //compute the sample and add it to our output
                        float sample = (float)Math.Cos(2 * Math.PI * freq * t);
                        samples.Add(sample * 0.75f);

                        
                        //adjust out T.
                        if (t >= 1) t -= 1;
                    }
    
                }
            }

            return samples.ToArray();
        }
        //NRZI encoding
        public static byte[] NRZIEncode(Byte[] bytes)
        {
            bool previousTone = true;

            List<Boolean> BitArray = new List<bool>();
            foreach (byte b in bytes)
            {
                for (int i = 0; i < 8 ; i++)
                {
                    bool bit = ((b & (byte)(1 << i)) != 0);
                    if (bit)
                    {
                        BitArray.Add(previousTone);
                    }
                    else
                    {
                        previousTone = !previousTone;
                        BitArray.Add(previousTone);                        
                    }
                    
                }
            }

            return BitsToBytesF(BitArray);

        }
        //Bit packing as per AX.25 specification.
        public static byte[] BitPack(Byte[] bytes)
        {
            int BitsInaRow = 0;
            List<Boolean> BitArray = new List<bool>();
            foreach (byte b in bytes)
            {
                for (int i = 0; i < 8; i++)
                {
                    bool bit = ((b & (byte)(1 << i)) != 0);
                    BitArray.Add(bit);

                    if (b == (Byte)AX25Deserializer.CONSTS.SEPARATOR)
                    {
                        BitsInaRow = 0;
                        continue;
                    }
 
                    if (bit && ++BitsInaRow == 5)
                    {
                        BitArray.Add(false);
                        BitsInaRow = 0;
                    }
                    else if (!bit)
                    {
                        BitsInaRow = 0;
                    }
                }
            }

            byte[] arr2 = BitsToBytesF(BitArray);

            return arr2;
        }
        //Convert bool list into a byte array.
        public static byte[] BitsToBytesF(List<Boolean> BitArray)
        {
            int bytesc = BitArray.Count / 8;
            if ((BitArray.Count % 8) != 0) bytesc++;
            byte[] arr2 = new byte[bytesc];
            int bitIndex = 0, byteIndex = 0;
            for (int i = 0; i < BitArray.Count; i++)
            {
                if (BitArray[i])
                {
                    arr2[byteIndex] |= (byte)(((byte)1) << (bitIndex));
                }
                bitIndex++;
                if (bitIndex == 8)
                {
                    bitIndex = 0;
                    byteIndex++;
                }
            }
            return arr2;
        }
    }
}
